#include <M5StickC.h>

#include "puppy.h"

void puppy_start() {
    angle_all_set(90, 90, 90, 90);  // servo 1	2	3	4	=>	set	90°
    delay(100);
}

void puppy_go() {
    angle_all_set(55, 60, 130, 120);  // servo 1	2	3	4
    delay(100);
    angle_all_set(55, 160, 130, 120);  // servo 1	2	3	4
    delay(100);
    angle_all_set(120, 130, 150, 60);  // servo 1	2	3	4
    delay(100);
    angle_all_set(120, 130, 60, 60);  // servo 1	2	3	4
    delay(100);
    angle_all_set(55, 130, 60, 60);  // servo 1	2	3	4
    delay(100);
    angle_all_set(55, 60, 130, 30);
    delay(100);
}

void puppy_back() {
    angle_all_set(120, 130, 60, 55);
    delay(100);
    angle_all_set(120, 130, 160, 55);
    delay(100);
    angle_all_set(60, 150, 130, 120);
    delay(100);
    angle_all_set(60, 60, 130, 120);
    delay(100);
    angle_all_set(60, 60, 130, 55);
    delay(100);
    angle_all_set(30, 130, 60, 55);
    delay(100);
}

// the setup routine runs once when M5StickC starts up
void setup() {
    // Initialize the M5StickC object
    M5.begin();

    // LCD display
    M5.Lcd.println(" ");
    M5.Lcd.println("  IIC_PUPPY");
    IIC_Init();  // sda  0     scl  26

    /*
          1              2
          3              4
    */
    angle_set(1, 90);  // servo 1	=>	set	90°
    angle_set(2, 90);
    angle_set(3, 90);
    angle_set(4, 90);
    delay(1000);
}

// the loop routine runs over and over again forever
/*
  1              2
  3              4
*/
uint8_t cnt = 10;
void loop() {
    while (cnt != 0) {
        cnt--;
        puppy_go();
    }
    cnt = 10;
    puppy_start();
    while (cnt != 0) {
        cnt--;
        puppy_back();
    }
    cnt = 10;
    puppy_start();
}
